/*
 * Copyright (c) 2012, Georgia Tech Research Corporation
 * All rights reserved.
 * @file RRT.cpp
 *
 * Authors:  Ana Huaman <ahuaman3@gatech.edu>, Tobias Kunz <tobias@gatech.edu>
 * Date: 10/2012
 *
 * Humanoid Robotics Lab      Georgia Institute of Technology
 * Director: Mike Stilman     http://www.golems.org
 *
 */

#include "PathPlanner.h"
#include "kinematics/Dof.h"
#include "kinematics/Joint.h"
#include "kinematics/TrfmTranslate.h"
#include "kinematics/TrfmRotateEuler.h"

/**
 * @function PathPlanner

 * @brief Constructor
 */
PathPlanner::PathPlanner() {
  copyWorld = false;
  world = NULL;
}

/**
 * @function PathPlanner
 * @brief Constructor
 */
PathPlanner::PathPlanner( robotics::World &_world, 
                          bool _copyWorld, double _stepSize ) {
  
  copyWorld = _copyWorld;
  
  if( copyWorld ) {
    printf( "Do not use this option yet \n" );
  } else {
    world = &_world;
  }
  
  stepSize = _stepSize;
}

/**
 * @function ~PathPlanner
 * @brief Destructor
 */
PathPlanner::~PathPlanner() {
  
  if( copyWorld ) {
    delete world;
  }
}

/**
 * Sets the Dofs for all the robots
 */
void PathPlanner::setRobotDofs(const Eigen::VectorXd& conf, const Eigen::VectorXi& links, std::vector<int>& robotSubset) {
	int offset1 = 0;
	int offset2 = 0;
	for (int i = 0; i < robotSubset.size(); i++) {
		robotics::Robot* robot = world->getRobot(robotSubset[i]);

		Eigen::VectorXd pose;
		Eigen::VectorXi link;
		pose.resize(robot->getQuickDofs().size());
		link.resize(robot->getQuickDofsIndices().size());
		for (int j = offset1; j < offset1+pose.size(); j++) {
			pose(j-offset1) = conf(j);
		}
		for (int j = offset2; j < offset2+link.size(); j++) {
			link(j-offset2) = links(j);
		}
		offset1 += pose.size();
		offset2 += link.size();
		robot->setDofs(pose, link);
	}
}

/**
 * @function planPath
 * @brief Main function
 */
bool PathPlanner::planPath( int _robotId,
							const Eigen::VectorXi &_links, 
                            const Eigen::VectorXd &_start, 
                            const Eigen::VectorXd &_goal, 
							std::vector<int>& robotSubset,
                            bool _bidirectional, 
                            bool _connect, 
                            bool _greedy,
							int _greedyRatio,
                            bool _smooth, 
                            unsigned int _maxNodes) {
	if (_robotId >= 0) { // single robot
		world->getRobot(_robotId)->setDofs( _start, _links );
	  if( world->checkCollision() )
		return false;
  
	  world->getRobot(_robotId)->setDofs( _goal, _links );
	  if( world->checkCollision() )
		return false;
	} else {
		// multi-robot
		setRobotDofs( _start, _links, robotSubset);
		if( world->checkCollision() )
			return false;
  
		setRobotDofs( _goal, _links, robotSubset );
		if( world->checkCollision() )
			return false;
	}

	bool result = true;
	  // now path to robot's goal state (if we didn't fail earlier)
		if( _bidirectional ) { 
			result = planBidirectionalRrt( _robotId, _links, _start, _goal, _connect, _greedy, _greedyRatio, _maxNodes, robotSubset ); 
		} else {
			result = planSingleTreeRrt( _robotId, _links, _start, _goal, _connect, _greedy,_greedyRatio, _maxNodes, robotSubset );
		}

		if (result && _smooth) {
			smoothPath( _robotId, _links, path, robotSubset );
		}
  
	  return result;
}


/**
 * @function planSingleRRT
 * @brief Finds a plan using a standard RRT
 */
bool PathPlanner::planSingleTreeRrt( int _robotId, 
                                     const Eigen::VectorXi &_links, 
                                     const Eigen::VectorXd &_start, 
                                     const Eigen::VectorXd &_goal, 
                                     bool _connect, 
                                     bool _greedy,
									 int _greedyRatio,
                                     unsigned int _maxNodes,
									 std::vector<int>& robotSubset) {
  
  RRT rrt( world, _robotId, _links, _start, stepSize );
  RRT::StepResult result = RRT::STEP_PROGRESS;
  
  double smallestGap = DBL_MAX;
  
  unsigned int greedyratio = 0;
 /* if(_greedy)
  {  
      std::cout << "Enter greedy ratio [1-10]:  ";
      std::cin>>greedyratio;
  }*/

  while ( result != RRT::STEP_REACHED && smallestGap > stepSize ) {

	  /** greedy section */
	  if( _greedy ) {
		  int prob = randomInRange(1,10);
		  //unsigned greedyratio = 1;  // X% of the time, be greedy
		  Eigen::VectorXd qtry = rrt.getRandomConfig(robotSubset);
		  if(_greedyRatio >= prob)
		  {
			  int NNIdx = rrt.getNearestNeighbor( qtry );
			  Eigen::VectorXd qnear = rrt.configVector[NNIdx];
			  Eigen::VectorXd u = (_goal - qnear);
			  Eigen::VectorXd diff = qtry-qnear;

			  //int ratio = 1;
			  if (u.norm()>diff.norm())
			  {
				  u = u/u.norm();
				  qtry = qnear + diff.norm()*u;
				  //qtry = qnear + (1-ratio)*diff + ratio*diff.norm()*u;
			  } else {
				  qtry = qnear + u;
			  }
		  }

		  /** greedy and connect */
		  if( _connect ) {
			  // ================ YOUR CODE HERE ===============
			  rrt.connect(qtry, robotSubset);
			  // ===============================================

		  /** greedy and NO connect */
		  } else {
			  // ================== YOUR CODE HERE =================== 
			  rrt.tryStep(qtry, robotSubset);
			  // =====================================================

		  }

		  /** NO greedy section */
	  } else {

		  /** NO greedy and Connect */
		  if( _connect ) {
			  rrt.connect(robotSubset);

			  /** No greedy and No connect -- PLAIN RRT */
		  } else {
			  rrt.tryStep(robotSubset);
		  }

	  }

	  if( _maxNodes > 0 && rrt.getSize() > _maxNodes ) {
		  printf("--(!) Exceeded maximum of %d nodes. No path found (!)--\n", _maxNodes );
		  return false;
	  }

	  double gap = rrt.getGap( _goal );
	  if( gap < smallestGap ) {
		  smallestGap = gap;
		  std::cout << "--> [planner] Gap: " << smallestGap << "  Tree size: " << rrt.configVector.size() << std::endl;
	  }
  } // End of while
  
    /// Save path  
  printf(" --> Reached goal! : Gap: %.3f \n", rrt.getGap( _goal ) );
  rrt.tracePath( rrt.activeNode, path, false );
  printf("\nSingle RRT Path Size: %d\n", path.size());
  return true;
}

/**
 * @function planBidirectionalRRT
 * @brief Grows 2 RRT (Start and Goal)
 */
bool PathPlanner::planBidirectionalRrt( int _robotId, 
                                        const Eigen::VectorXi &_links, 
                                        const Eigen::VectorXd &_start, 
                                        const Eigen::VectorXd &_goal, 
                                        bool _connect,
                                        bool _greedy, // no effect here
										int _greedyRatio,
                                        unsigned int _maxNodes,
										std::vector<int>& robotSubset) {
  
  // ============= YOUR CODE HERE ======================
  // HINT: Remember trees grow towards each other!

  RRT rrta( world, _robotId, _links, _start, stepSize );
  RRT rrtb( world, _robotId, _links, _goal, stepSize );
  
 
  RRT::StepResult result = RRT::STEP_PROGRESS;
  double smallestGap = DBL_MAX;

  printf("\nBidirectional starting!\n");
  int count = 0;
  int NNIdx = 0;
  Eigen::VectorXd qnear;
  double gap;
  while ( result != RRT::STEP_REACHED && smallestGap > stepSize ) {
	  if(_connect) // CONNECT (for a single random config)
	  {
		  if(count%2==0)
		  {
			  //printf("\n********TREE A********\n");
			  				 rrta.connect(robotSubset);

			  NNIdx = rrtb.getNearestNeighbor(rrta.configVector[rrta.activeNode]);
			  Eigen::VectorXd qnear = rrtb.configVector[NNIdx];

			  rrta.connect(qnear, robotSubset);
			  //std::cout<<"\ngetting gap A"<<std::endl;
			  gap = rrta.getGap( qnear ); 
			  //std::cout<<"\n GOT gap A: "<< gap << "\n"<<std::endl;
		  } else {
			  //printf("\n--------TREE B--------\n");
			  
				  rrtb.connect(robotSubset);
				  NNIdx = rrta.getNearestNeighbor(rrtb.configVector[rrtb.activeNode]);
			  Eigen::VectorXd qnear = rrta.configVector[NNIdx];

			  rrtb.connect(qnear, robotSubset);
			  //std::cout<<"\ngetting gap B"<<std::endl;
			  gap = rrtb.getGap( qnear ); 
			  //std::cout<<"\n GOT gap B: "<<gap<<"\n"<<std::endl;
		  }
	  } else { //NO CONNECT
		  if(count%2==0)
		  {
			  //printf("\n********TREE A********\n");
			  rrta.tryStep(robotSubset);
			  NNIdx = rrtb.getNearestNeighbor(rrta.configVector[rrta.activeNode]);
			  Eigen::VectorXd qnear = rrtb.configVector[NNIdx];

			  rrta.connect(qnear, robotSubset);
			  //std::cout<<"\ngetting gap A"<<std::endl;
			  gap = rrta.getGap( qnear ); 
			  //std::cout<<"\n GOT gap A: "<< gap << "\n"<<std::endl;
		  } else {
			  //printf("\n--------TREE B--------\n");
			  rrtb.tryStep(robotSubset);
			  NNIdx = rrta.getNearestNeighbor(rrtb.configVector[rrtb.activeNode]);
			  Eigen::VectorXd qnear = rrta.configVector[NNIdx];

			  rrtb.connect(qnear, robotSubset);
			  //std::cout<<"\ngetting gap B"<<std::endl;
			  gap = rrtb.getGap( qnear ); 
			  //std::cout<<"\n GOT gap B: "<<gap<<"\n"<<std::endl;
		  }
	  }
	  
	  //printf("treesize (A): %d treesize (B): %d TOTAL: %d NNIdx: %d\n", rrta.getSize(), rrtb.getSize(), rrta.getSize()+rrtb.getSize(), NNIdx);
	  if( _maxNodes > 0 && rrta.getSize() + rrtb.getSize() > _maxNodes ) {
		  printf("--(!) Exceeded maximum of %d nodes. No path found (!)--\n", _maxNodes );
		  return false;
	  }
	  //std::cout<<"I am here 1"<<std::endl;

	  if( gap < smallestGap ) {
		  //std::cout<<"I am here 2"<<std::endl;
		  smallestGap = gap;
		  std::cout << "--> [planner] Gap: " << smallestGap << "  Tree size: " << rrta.configVector.size()+rrtb.configVector.size() << std::endl;
	  }
	  //std::cout<<"I am here 3 count: "<< count << "\n" <<std::endl;
	  count++;
  } // end while
  std::cout<<"--Exiting WHILE loop" <<std::endl;
 /* 
 printf("\n --> Reached goal! " );
 printf("\n --> PATH DONE! " );
 printf("\nBI-directional RRT Path Size: %d\n", path.size());*/

  rrta.tracePath(rrta.activeNode, path, false);
  rrtb.tracePath(rrtb.activeNode, path, true);

  printf(" --> Reached goal! : Gap: %.3f \n", gap);
  printf("\nBi-Directional RRT Path Size: %d\n", path.size());
  return true;
  // ===================================================	
  
}


/**
 * @function checkPathSegment
 * @brief True iff collision-free
 */
bool PathPlanner::checkPathSegment( int _robotId, 
                                    const Eigen::VectorXi &_links, 
                                    const Eigen::VectorXd &_config1, 
                                    const Eigen::VectorXd &_config2,
									std::vector<int>& robotSubset) {
  
  int n = (int)((_config2 - _config1).norm() / stepSize );
  
  for( int i = 0; i < n; i++ ) {
    Eigen::VectorXd conf = (double)(n - i)/(double)n * _config1 + (double)(i)/(double)n * _config2;
    if (_robotId >= 0) { // single robot
		world->getRobot(_robotId)->setDofs( conf, _links );
		world->getRobot(_robotId)->update();
	} else {
		// multi-robot
		const Eigen::VectorXd& t_conf = conf;
		setRobotDofs(t_conf, _links, robotSubset);
		for (int i = 0; i < robotSubset.size(); i++) {
			world->getRobot(robotSubset[i])->update();
		}
	}
    if( world->checkCollision() ) {
      return false;
    }
  }
  
  return true;
}

/**
* @function smoothPath
*/
void PathPlanner::smoothPath( int _robotId,
 
                              const Eigen::VectorXi &_links, 
 
                              std::list<Eigen::VectorXd> &_path,
							  std::vector<int>& robotSubset) {
 
/*std::cout << "\n\n*********************Quick DOF's***************\n" << std::endl;
int hold;
std::cout << "Pausing........"<< std::endl;
std::cin >> hold;
std::cout << "Continuing........"<< std::endl;
std::cout << world->getRobot(_robotId)->getQuickDofs() << std::endl;
world->getRobot(_robotId)->setPositionXYZ(1.0, 2.0, 3.0);
world->getRobot(_robotId)->update();
std::cout << "\n\n************************************\n" << std::endl;*/
  
  
// =========== YOUR CODE HERE ==================
// HINT: Use whatever technique you like better, first try to shorten a path and then you can try to make it smoother
  
std::cout << "(!) ---Shortening Path---" << std::endl;
std::cout << "Path Size: " << _path.size() << std::endl;
int oldPathSize = _path.size();
 
if(_path.size() < 3)// Path cannot be shortened base case
{
  printf("(!)--Path of length %d cannot be shortened", _path.size());
  return;
}
std::list<Eigen::VectorXd>::iterator iA = _path.begin(); //start node
std::list<Eigen::VectorXd>::iterator iB = _path.begin();
std::list<Eigen::VectorXd>::iterator iC = _path.begin();
 
advance(iB, 1); // node being skipped to check if we can delete it
advance(iC, 2); // end node
 
bool noCollision = false;
bool stop = false;
int oldsize = oldPathSize;
 
while(!stop)
{
  //std::cout << "(!)--Shortening... " << oldsize << std::endl;
  while(iC != _path.end()) //Continue shortening until the end node reaches path end
  {
 	  //std::cout << "\nChecking for collisions... ";
 	  //Check if we can delete every other node....1,3,5,7,9
 	  noCollision = checkPathSegment(_robotId, _links, *iA, *iC, robotSubset);
 	  if (noCollision) // No collision from A-->C so we can delete B
 	  {
 	 	  //std::cout << " deleting: ";
 	 	  iB = _path.erase(iB); // Erase node B (B now points to C node)
 	 	  advance(iB,1); // Set B +1 to the new node to check delete
 
 	  } else // A-/->C so increment all iterators by 2 and repeat process
 	  {
 	 	  //std::cout << " SKIPPING: ";
 	 	  advance(iB, 2);
 	  }
 	  //std::cout << "path size: " << _path.size();
 	  //std::cout << " (" << distance(iC, _path.end()) << ")";
 
 	  iA = iC;
 	  if( distance(iC, _path.end()) < 2 )
 	  {
 	 	  //std::cout << " BREAK!" << std::endl;
 	 	  iC = _path.end();
 	  } else {
 	 	  advance(iC,2);
 	  }
  } // end of nested while
 
  if(_path.size() == oldsize || _path.size() < 3 ) // No nodes removed so done. Also, if length is less than 3, then there is nothing to shorten
  {
 	  stop = true;
  } else { // Nodes were removed, so repeat the process with this new _path
 
 	  oldsize = _path.size();
 
 	  //std::cout << "\n(!) Resetting iterators... ";
 	  iA = _path.begin();
 	  iB = _path.begin();
 	  iC = _path.begin();
 	   
 	  advance(iB,1);
 	  if( distance(iC, _path.end()) < 2)
 	  {
 	 	  //std::cout << " Path Size too Small to repeat!" << std::endl;
 	 	  iC = _path.end();
 	  } else {
 	 	  //std::cout << " Done!" << std::endl;
 	 	  advance(iC,2);
 	  }
  }
} // end while loop = Path has been shortened significantly!
 
 
std::cout << "\n\n(!) Old Path: " << oldPathSize << " NEW Final Path Size: " << _path.size() << std::endl;
std::cout << "(!) Connecting straight lines between RRT nodes" << std::endl;
 
//Outputs the vector of joint angles to go to
/* std::list<Eigen::VectorXd>::iterator ip;
int pcount = 1;
for (ip = _path.begin(); ip!=_path.end(); ++ip)
{
  std::cout << "Path: " << pcount << std::endl << std::endl;
  std::cout << *ip;
  std::cout << std::endl << std::endl;
 
  ++pcount;
}*/
 
std::list<Eigen::VectorXd>::iterator p = _path.begin();
Eigen::VectorXd _start = *p;
RRT rrt( world, _robotId, _links, _start, stepSize );
  
for(p=_path.begin(); p!=_path.end(); ++p)
{
  rrt.connectTree(*p, rrt.activeNode, robotSubset );
}
 
std::cout << "(!) Tracing new path...";
_path.clear();
rrt.tracePath(rrt.activeNode, _path, false);
std::cout << " Done!... ";
std::cout << " RRT size: " << rrt.configVector.size() << " Path size: " << _path.size() << std::endl;
return;
// ========================================	 
 
 
}


